﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;

namespace WPF.DEMO.Pages
{
    /// <summary>
    /// KalmanPage.xaml 的交互逻辑
    /// </summary>
    public partial class KalmanPage : Page
    {
        public KalmanPage()
        {
            InitializeComponent();
        }

        public WriteableBitmap Bitmap
        {
            get { return (WriteableBitmap)GetValue(BitmapProperty); }
            set { SetValue(BitmapProperty, value); }
        }

        public static readonly DependencyProperty BitmapProperty =
            DependencyProperty.Register("Bitmap", typeof(WriteableBitmap), typeof(KalmanPage), new PropertyMetadata(default(WriteableBitmap)));

        private Random random;
        private SharpSoft.Maths.KalmanFilter2 kalmanFilter2;
        private async void btn_start_Click(object sender, RoutedEventArgs e)
        {
            Bitmap = new WriteableBitmap(1920, 1080, 96, 96, PixelFormats.Pbgra32, BitmapPalettes.WebPalette);
            random = new Random();
            kalmanFilter2 = new SharpSoft.Maths.KalmanFilter2()
            {
                InitialPosition = new System.Numerics.Vector2(0, 500),
                MeasurementCovariance = (float)num_MeasurementCovariance.Value,
                InitialVariance = (float)num_InitialVariance.Value
            };
            kalmanFilter2.Reset();

            float x = 0f, y = 500f;
            for (int i = 0; i < 200; i++)
            {

                var p = new System.Numerics.Vector2(x, y);
                var kp = kalmanFilter2.Update(p, DateTime.Now.Ticks);
                DrawPoint(p, Color.FromArgb(150, 255, 0, 0));
                DrawPoint(kp, Color.FromArgb(150, 0, 0, 255));
                x += (float)random.NextDouble() * 5 + 3;
                y += random.Next(-10, 10);
                await Task.Delay(random.Next(10, 100));
            }
        }

        void DrawPoint(System.Numerics.Vector2 p, Color color)
        {
            var size = new System.Numerics.Vector2(4, 4);
            Bitmap.FillRectangle((int)p.X, (int)p.Y, (int)(p.X + size.X), (int)(p.Y + size.Y), color);
        }

        private void canvas_SizeChanged(object sender, SizeChangedEventArgs e)
        {

        }
    }
}
